function ret = f1_vector(t,q, a, ep)
global tp tp0 yact ydes p

ndof = length(q)/2;
dx = q(ndof+1:end);
% q
% pause(1000)
M    = D_mat(q);
C    = C_mat(q);
G    = G_vec(q);

% pause(0.5);

%%%Outputs
y_a1     = ya1_sca(q, p, a);
y_a2     = ya2_vec(q, p, a);
y_d1     = yd1_sca(q, p,  a);
y_d2     = yd2_vec(q, p,  a);

%%%Outputs
y1 = y_a1 - y_d1;
y2 = y_a2 - y_d2;


y_a = [y_a1; y_a2];
y_d = [y_d1; y_d2];

%%%Jacobians of Outputs
Dy_a1    = Dya1_mat(q, p, a);
Dy_d1    = Dyd1_mat(q, p, a);
Dy_a2    = Dya2_mat(q, p, a);
Dy_d2    = Dyd2_mat(q, p, a);

Dy_1 = Dy_a1 - Dy_d1;
Dy_2 = Dy_a2 - Dy_d2;

%%%Control Fields
vf    = [dx; M \ (-C*dx - G)];
B_IO  = eye(ndof);
gf    = [zeros(size(B_IO)); M \ B_IO];

%%%Lie Derivatives
Lgy1 = Dy_1*gf;
Lgy2 = Dy_2*gf;
Lfy1 = Dy_1*vf;
Lfy2 = Dy_2*vf;

%%%Second Order Jacobians

DLfy_a2  = DLfya2_mat(q, p, a);
DLfy_d2  = DLfyd2_mat(q, p, a);

DLfy2 = DLfy_a2-DLfy_d2;

%%%Second Lie Derivatives

LfLfy2 = DLfy2*vf;
LgLfy2 = DLfy2*gf;

tp = [tp, t+tp0];
yact = [yact, y_a];
ydes = [ydes, y_d];

% Vector field

A = [Lgy1; LgLfy2];
u = -A \ ([0; LfLfy2]+[Lfy1; 2*ep*Lfy2]+ [2*ep*y1; ep^2*y2]);

% Lfy   = (Dy_a - Dy_d) * vf;
% LfLfy = (DLfy_a - DLfy_d) * vf;
% A = (DLfy_a - DLfy_d)*gf; 
% % min(svd(A))
% y = [0; y_a(2:end) - y_d(2:end)];
% 
% u = -A \ (LfLfy + ep * diag([1, 2*ones(1,3)]) * Lfy + ep^2 * y); 
% % u = zeros(4,1);

ret = vf + gf * u;

end
